import json
import sys
from math import cos, degrees, radians, sqrt, atan2

import numpy as np
from PyQt5.QtWidgets import QApplication, QWidget, QFormLayout, QLineEdit, QPushButton, QTextEdit, QComboBox, QLabel

maxY = 65

RATIO = 3.0
point_o = tuple(map(lambda i: i * RATIO, (0.0, 0.0)))
point_b = tuple(map(lambda i: i * RATIO, (13.0, 0.0)))
point_e = tuple(map(lambda i: i * RATIO, (5.0, 66.0)))

theta = 119.0

link_ob = 13.0 * RATIO
link_bc = 27.0 * RATIO
link_oa = 27.0 * RATIO
link_ad = 35.0 * RATIO
link_cd = 27.0 * RATIO
link_de = 30.0 * RATIO
link_ae = sqrt(link_ad ** 2 + link_de ** 2 - 2 * link_ad * link_de * cos(radians(theta)))


def calc_radians_between_vector(a, b):
    return atan2(a[0] * b[1] - a[1] * b[0], a[0] * b[0] + a[1] * b[1])


def calculate_intersection(p0, p1, r0, r1):
    # 圆心距离
    d = sqrt(sum(map(lambda i, j: (i - j) ** 2, p0, p1)))
    # 半径之和
    r = r0 + r1

    # 一个圆在另一个圆内，则没有交点
    if d < abs(r0 - r1):
        return None

    # 如果两个圆相离，则没有交点
    if d > r:
        return None

    # 如果两个圆相切，则有一个交点
    if d == r:
        return (p0[0] + (p1[0] - p0[0]) * (r0 / r), p0[1] + (p1[1] - p0[1]) * (r0 / r)),

    # 如果两个圆相交，则有两个交点
    a = (r0 ** 2 - r1 ** 2 + d ** 2) / (2 * d)

    h0 = sqrt(r0 ** 2 - a ** 2)

    p2 = (p0[0] + (p1[0] - p0[0]) * a / d, p0[1] + (p1[1] - p0[1]) * a / d)
    p3 = (p2[0] + h0 * (p1[1] - p0[1]) / d, p2[1] - h0 * (p1[0] - p0[0]) / d)
    p4 = (p2[0] - h0 * (p1[1] - p0[1]) / d, p2[1] + h0 * (p1[0] - p0[0]) / d)

    return p3, p4


def inverse(e):
    e = tuple(map(lambda i: i * RATIO, e))
    # 计算左交点
    intersection_left_points = calculate_intersection(point_o, e, link_oa, link_ae)
    if intersection_left_points is None:
        # 求左交点失败
        return

    point_a = intersection_left_points[0]
    if len(intersection_left_points) == 2:
        if intersection_left_points[1][0] < intersection_left_points[0][0]:
            point_a = intersection_left_points[1]

    # 计算点D位置
    d_points = calculate_intersection(point_a, e, link_ad, link_de)
    if d_points is None:
        # 求交点D失败
        return None

    vec_ae = tuple(map(lambda i, j: i - j, e, point_a))
    vec_ad = tuple(map(lambda i, j: i - j, d_points[0], point_a))
    v0 = calc_radians_between_vector(vec_ae, vec_ad)
    vec_ad = tuple(map(lambda i, j: i - j, d_points[1], point_a))
    v1 = calc_radians_between_vector(vec_ae, vec_ad)
    point_d = d_points[0] if v1 > v0 else d_points[1]

    # 计算右交点
    intersection_right_points = calculate_intersection(point_d, point_b, link_cd, link_bc)
    if intersection_right_points is None:
        # 求右交点失败
        return

    point_c = intersection_right_points[0]
    if len(intersection_right_points) == 2:
        if intersection_right_points[1][0] > intersection_right_points[0][0]:
            point_c = intersection_right_points[1]

    vec_oa = tuple(map(lambda i, j: i - j, point_o, point_a))
    vec_x_negative = (-1, 0)
    vec_bc = tuple(map(lambda i, j: i - j, point_c, point_b))
    vec_x_positive = (1, 0)

    alpha = calc_radians_between_vector(vec_oa, vec_x_positive)
    beta = calc_radians_between_vector(vec_bc, vec_x_negative)
    return degrees(alpha), degrees(beta)


def arccos(value):
    eps = 0.5
    if 1.0 < value < 1.0 + eps:
        value = 1.0
    elif -1.0 - eps < value < -1.0:
        value = -1.0
    try:
        # print(value)
        return np.arccos(value)
    except Exception as e:
        print("无效的值：", value)
        return None


def caculatePoint(t, xs, xf, h, d1, d2, d3, d4):
    """
    步态分析
    :param t:  当前时刻
    :param xs: 末端起始坐标
    :param xf: 末端结束坐标
    :param h:  抬腿高度
    :return: 当前的x,y
    """
    T = 1
    faai = 0.5

    if t < faai * T:  # 前半个周期
        sigma = 2 * np.pi * t / (faai * T)
        # 摆动腿
        xexp_baidong = (xf - xs) * (sigma - np.sin(sigma)) / (2 * np.pi) + xs
        # 支撑腿
        xexp_zhicheng = (xs - xf) * (sigma - np.sin(sigma)) / (2 * np.pi) + xf
        # 抬腿高度
        zexp = h * (1 - np.cos(sigma)) / 2

        x1 = xexp_baidong * d1
        x2 = xexp_zhicheng * d2
        x3 = xexp_zhicheng * d3
        x4 = xexp_baidong * d4

        y1 = maxY + 5 - zexp
        y2 = maxY
        y3 = maxY + 5
        y4 = maxY - zexp

    elif t >= faai * T and t <= T:  # 后半个周期
        sigma = 2 * np.pi * (t - faai * T) / (faai * T)
        # 摆动腿
        xexp_baidong = (xf - xs) * (sigma - np.sin(sigma)) / (2 * np.pi) + xs
        # 支撑腿
        xexp_zhicheng = (xs - xf) * (sigma - np.sin(sigma)) / (2 * np.pi) + xf
        # 抬腿高度
        zexp = h * (1 - np.cos(sigma)) / 2

        x1 = xexp_zhicheng * d1
        x2 = xexp_baidong * d1
        x3 = xexp_baidong * d1
        x4 = xexp_zhicheng * d1

        y1 = maxY + 5
        y2 = maxY - zexp
        y3 = maxY + 5 - zexp
        y4 = maxY

    return x1, y1, x2, y2, x3, y3, x4, y4


class MainWindow(QWidget):
    def __init__(self):
        super(MainWindow, self).__init__()

        self.setWindowTitle('步态指令生成器 - 黑马程序员 - https://gitee.com/tangyang/leg-kinematics - v1.0')
        self._direction_index = 0

        layout = QFormLayout()
        self.setLayout(layout)

        self.le_start_x = QLineEdit("-10")
        layout.addRow('起始X位置', self.le_start_x)

        self.le_end_x = QLineEdit("20")
        layout.addRow('结束X位置', self.le_end_x)

        self.le_height = QLineEdit("30")
        layout.addRow('抬起高度', self.le_height)

        self.cb_direction = QComboBox()
        self.cb_direction.addItem("前进")
        self.cb_direction.addItem("后退")
        self.cb_direction.addItem("向左")
        self.cb_direction.addItem("向右")
        self.cb_direction.currentIndexChanged.connect(self.direction_selection_changed)
        layout.addRow("运动方向", self.cb_direction)

        self.le_delay = QLineEdit("50")
        layout.addRow("延时时间", self.le_delay)

        btn_gen = QPushButton('生成指令')
        btn_gen.clicked.connect(self.click_gen)
        layout.addRow(btn_gen)

        self.te_json = QTextEdit()
        layout.addRow('生成的指令', self.te_json)

        self.lb_tip = QLabel()
        layout.addWidget(self.lb_tip)

    def direction_selection_changed(self, i):
        self._direction_index = i

    def click_gen(self):
        self.lb_tip.setText("")
        start_x = self.le_start_x.text()
        try:
            start_x = int(start_x)
        except:
            self.lb_tip.setText("起始X位置不能为空，必须说数字")
            return

        end_x = self.le_end_x.text()
        try:
            end_x = int(end_x)
        except:
            self.lb_tip.setText("结束X位置不能为空，必须说数字")
            return

        height = self.le_height.text()
        try:
            height = int(height)
        except:
            self.lb_tip.setText("抬起高度不能为空，必须说数字")
            return

        delay = self.le_delay.text()
        try:
            delay = int(delay)
        except:
            self.lb_tip.setText("延时时间不能为空，必须说数字")
            return

        d1, d2, d3, d4 = -1, -1, -1, -1
        if self._direction_index == 1:
            d1, d2, d3, d4 = 1, 1, 1, 1
        elif self._direction_index == 2:
            d1, d2, d3, d4 = -1, -1, 1, 1
        elif self._direction_index == 3:
            d1, d2, d3, d4 = 1, 1, -1, -1

        data = []
        t = 0
        for i in range(5):
            point = caculatePoint(t, start_x, end_x, height, d1, d2, d3, d4)
            t += 0.2

            rf = tuple(map(lambda i: int(i), inverse((point[0], point[1]))))
            rb = tuple(map(lambda i: int(i), inverse((point[2], point[3]))))
            lf = tuple(map(lambda i: int(i), inverse((point[4], point[5]))))
            lb = tuple(map(lambda i: int(i), inverse((point[6], point[7]))))
            cmd = [lf[0], lf[1], lb[0], lb[1], rf[0], rf[1], rb[0], rb[1], delay]

            data.append(cmd)

        action = {
            "type": 3,
            "count": 30,
            "list": data
        }

        result = json.dumps(action).encode("utf-8")
        self.te_json.setText(result.decode('utf-8'))


if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    exec_ = app.exec_()
    # 窗口关闭 停止线程中的循环
    window.has_closed = True
    sys.exit(exec_)
